delete(instrfindall);
g=serial('com10');                                     
set(g,'BaudRate',115200,'StopBits',1,'Parity','none','DataBits',8,'InputBufferSize',8000);
fopen(g);
fileprefix = 'anchor1_10cm';

P = [0,0,1000];
x3=0;
y3=600;
z3=0; 
p1=[x3-650 y3-600 z3];
p2=[x3+650 y3-600 z3];
p3=[x3 y3 z3];          
bases = [p1;p2;p3];

measurementLength =1000;
distances =zeros(3,measurementLength);
measurementIndeices = zeros(3,1);
indicesSet = [1,2,3];
while(true)
  str  = fscanf(g,'%s');
  a    = sscanf(str,'distance%d:%dmm,');
  if length(a)==2
    % disp(a);
    measurementIndeices(a(1)) = measurementIndeices(a(1))+1;
    if(measurementIndeices(a(1))>measurementLength)
      save(strcat(fileprefix,'_',num2str(round(now*1e6)),'.mat'));
      break;
    else
      distances(a(1),measurementIndeices(a(1)))= a(2);
    end
    if(min(measurementIndeices)== measurementIndeices(a(1)))
      % posistion determination 
      ranges=distances(:,measurementIndeices(a(1)));
      % posSolve(bases,ranges,initp);
      P0 = P;
      % options=optimset('MaxFunEvals',1e20,'TolFun',1e-18,'MaxIter',300,'display','iter');
      options=optimset('MaxFunEvals',1e20,'TolFun',1e-18,'MaxIter',300,'display','off');
      [P,fval] = fsolve(@(x)rangingEquation(x,bases,ranges),P0,options);
      res = rangingEquation(P,bases,ranges);
      disp(P);
      disp(res);
      % pause();
    end
  elseif length(a)~=0
    warning('there are some mistakes');
  end
%   pause(1);
end
